/**
 * IK Arm
 * Based on Reach 1. 
 * Based on code from Keith Peters (www.bit-101.com)
 * 
 * The arm follows the position of the mouse by
 * calculating the angles with atan2(). 
 */

import processing.serial.*;

// String portname = "/dev/tty.usbserial-A5002rKW"; // james arduino
  String portname = "/dev/tty.usbserial-A1001NQr"; // thomas' arduino, w/ feet
// String portname = "/dev/tty.SerialPort-1"; // for testing
Serial port;  // Create object from Serial class


float z = 100;
float x = 100;
float y = 100;
float x2 = 100;
float z2 = 100;
float y2 = 100;
float segLength = 50;

float zFakMaux = 150;  // for moving w/o mouse
float zCanvasLoc = 155;  // z loc of piece to cut
float xCanvasWidth = 100;  // width of canvas
float xFakMaux = 0;  // for moving w/o mouse - starts from 0, +/- width
float FakStart = 20;
float FakMax = 80;
float yFakMaux = FakStart;
int FakCount = 0;
int FakCountMax = 6;

void setup() {
  size(400, 300);
  smooth(); 
  strokeWeight(1.0);
  stroke(0, 100);
  PFont fontA = loadFont("Ziggurat-HTF-Black-32.vlw");
  textFont(fontA, 12);
  
  // init serial
  port = new Serial(this, portname, 9600);
}

void draw() {
  background(200);
  float dx = xFakMaux - x; 
  float dz = zFakMaux - z; // z = 100, 200x200 sketch, change the coord. system
  float dy = yFakMaux - y;
  float angle1 = atan2(dy, dz);   // calculates in radians pi to - pi
  
  float tz = zFakMaux - cos(angle1) * segLength;
  float ty = yFakMaux - sin(angle1) * segLength;
  dz = tz - z2;  // z2 is the center of the matrix
  dy = ty - y2;
  float angle2 = atan2(dy, dz);  
  z = z2 + cos(angle2) * segLength;
  y = y2 + sin(angle2) * segLength;
  
  segment(z, y, angle1); 
  segment(z2, y2, angle2); 
  segment(z + cos(angle1) * 50, y + sin(angle1) * 50, radians(0));  // radians is the angle of the wrist 

/*   
  pushMatrix();

//  translate(z, y); // redefine the origin
  translate(100, 100); // redefine the origin
  rotate(angle2);       // rotate in radians
  line(0, 0, segLength, 0);  // draw line the seglength
  
  pushMatrix();
  translate(z2 - 100, y2 - 100); // redefine the origin
  rotate(angle1);       // rotate in radians
  line(0, 0, segLength, 0);  // draw line the seglength
  
  
  
  popMatrix();
  popMatrix();
 // popMatrix();
  */
  
  //  segment(z2, y2, -angle2-angle1); 
  
  
  // output angles
  fill(100);
  text("angle1=" + degrees(angle1), 50, 120);
  text("angle2=" + degrees(angle2), 50, 135);
  text("dz=" + dz, 50, 150);
  text("dx=" + dx, 50, 165);
  text("dy=" + dy, 50, 165);
  text("zFakMaux=" + zFakMaux, 50, 180);
  text("yFakMaux=" + yFakMaux, 50, 195);


  
  
  int servo190 = 3030; // servo1 at 90    angle 0 in calc
  int servo100 = 1280; // servo1 at 0 
  int servo290 = 4860; // servo2 at 90 down  180 deg in calc
  int servo200 = 3210; // level with other seg
  int servo2i90 = 1560; // -90 up
  

              text("AngleOnly" + (( -1 * (degrees(angle2) - degrees(angle1)))) , 50, 215);
              text("AngleWrist" + ((-1 * (degrees(angle1)))) , 50, 230);  // wrist calc -angle1 (opposite of elbow)
   //    text("calcAngle" + nf(((int)((servo290 - servo200) / 90 * -1 * (degrees(angle2) - degrees(angle1))) + servo200), 4) , 50, 245);
  

       // set servo defaults
       port.write("s03000       s33140     ");
       // move servo 1
       port.write("s1" + nf((int)(servo100 + (servo190 - servo100) / 90 * -1 * degrees(angle2)), 4));
       // move servo 2 NOTE angle2 - angle1
       port.write("s2" + nf(((int)((servo290 - servo200) / 90 * -1 * (degrees(angle2) - degrees(angle1))) + servo2i90), 4));
       // move servo 2 NOTE angle2 - angle1
       // port.write("s2" + nf(((int)((servo290 - servo200) / 90 * -1 * (degrees(angle2) - degrees(angle1))) + servo2i90), 4));

      // code for wrist might be 
      // shoulder -angle2
      // elbow -angle2-angle1
      // wrist angle2+angle1
      // negation possibly because of mirrored env
  yFakMaux++;
  xFakMaux+= .5; // for smoother response
  if (yFakMaux >= FakMax) {
      yFakMaux = FakStart;
      xFakMaux = 0;
      line(155, 100, 155, 0); // draw line for comparison    
      delay(100);
  } else if (yFakMaux <= 40 && yFakMaux > 30) {  // knife in
        zFakMaux++;
        // xFakMaux++;
      } else if (yFakMaux >= 60 && yFakMaux < 70) {  // knife out
        zFakMaux--;
        // xFakMaux--;
      }
  line(zCanvasLoc, 100, zCanvasLoc, 0); // draw line for comparison
//  line(xFakMaux, 100, xFakMaux, 0); // draw line for comparison

  // handle arc and of arm for x movement across the canvas, from left to right
  fill(255, 0, 0);
  stroke(255, 0, 0);
  int xOffset = 300;
  int yOffset = 100;
  line(xOffset - xCanvasWidth/2, yOffset + zCanvasLoc, xOffset + xCanvasWidth/2 , yOffset + zCanvasLoc);  //canvas for ref
 // line(xOffset, yOffset, xOffset + xFakMaux, yOffset + zCanvasLoc); //from origin to ref point
  
  // float angle3 = atan2(xFakMaux, zCanvasLoc);  
  float angle3 = -atan2(xFakMaux, zCanvasLoc) + radians(90);  
  x = cos(angle2) * zCanvasLoc;
  float yCan = sin(angle2) * zCanvasLoc;
  // for ref
   stroke(1, 255, 0);
  line(xOffset, yOffset, xOffset + xFakMaux, yOffset + zCanvasLoc); //from origin to ref point
 
  stroke(0, 0, 255);
  
  pushMatrix();
  translate(xOffset, yOffset); // redefine the origin
  rotate(angle3);       // rotate in radians
  line(0, 0, zCanvasLoc, 0);  // draw line the seglength
  popMatrix();

  // trace
  text("x " +x , 50, 260);
  text("angle3 " + degrees(angle3) , 50, 275);
  text("xFakMaux " + xFakMaux , 50, 290);
  
  
  // clean up
  delay(100); // slow to visualize
  if (yFakMaux == FakStart + 2) {
    delay(1000);
  }
  stroke(0, 100);
  
}

void segment(float x, float y, float a) {
  pushMatrix();
  translate(x, y); // redefine the origin
  rotate(a);       // rotate in radians
  line(0, 0, segLength, 0);  // draw line the seglength
  popMatrix();
}

